#!/usr/bin/python3
import rospy
import serial
import struct
from yolo_pkg.msg import Yolo


emotion = ""                        #储存当前识别到的程序
confidence = 0                      #当前置信度
detect_flag = 0                     #识别标志，默认0
start_time = 0                      #开始时间
happy_count = 0                     #识别到开心的次数
sad_count = 0                       #识别到伤心的次数
angry_count = 0                     #识别到生气的次数

act_state = 0                       #当前状态
detect_state = 0                    #是否开启识别


#串口解包
def read_packet():
    packet = bytearray()
    while not rospy.is_shutdown():
        byte = ser.read(1) 
        if byte:

            if byte == b'\xAA':
                packet = bytearray([0xAA])
            elif packet and byte != b'\xFF':
                packet.append(ord(byte))
            elif byte == b'\xFF' and packet:
                packet.append(0xFF)
                print(f"Received packet: {packet.hex()}")
                data = packet[1:-1]
                break
        else:
            rospy.sleep(0.01)  
    return data


#识别到情绪回调函数
def emotion_callback(emo):
    global emotion, confidence,detect_state
    emotion = emo.Class
    confidence = emo.Confidence
    if confidence >= 0.3:
        emotion_mp3(30, 20)
    
#串口控制mp3
def mp3_control(data):
    global flag,act_state,detect_state
    print(f"Returned data: {data.hex()}")
    
    if data and data[0] == 0x01:
        send_arr = bytearray([0xAA, 0x05, 0x00, 0xAF]) 
        send_arr = struct.pack("%dB"%len(send_arr), *send_arr)
        print(f"Sending response: {send_arr.hex()}")
        ser_send.write(send_arr)
        if act_state == 0:
            send_arr = bytearray([0xAA, 0x03, 0x00, 0xAD]) 
            send_arr = struct.pack("%dB"%len(send_arr), *send_arr)
        # flag = 1
    if data and data[0] == 0x02:
        send_arr = bytearray([0xAA, 0x14, 0x00, 0xBE]) 
        send_arr = struct.pack("%dB"%len(send_arr), *send_arr)
        print(f"Sending response: {send_arr.hex()}")
        ser_send.write(send_arr)
    if data and data[0] == 0x07:
            send_arr = bytearray([0xAA, 0x02, 0x00, 0xAC]) 
            send_arr = struct.pack("%dB"%len(send_arr), *send_arr)
            print(f"Sending response: {send_arr.hex()}")
            print("open")
            ser_send.write(send_arr)
            act_state = 1
    if data and data[0] == 0x06:
            send_arr = bytearray([0xAA, 0x03, 0x00, 0xAD]) 
            send_arr = struct.pack("%dB"%len(send_arr), *send_arr)
            print(f"Sending response: {send_arr.hex()}")
            print("close")
            ser_send.write(send_arr)
            act_state = 0
    if data and data[0] == 0x04:
        send_arr = bytearray([0xAA, 0x15, 0x00, 0xBF]) 
        send_arr = struct.pack("%dB"%len(send_arr), *send_arr)
        print(f"Sending response: {send_arr.hex()}")
        ser_send.write(send_arr)

    if data and data[0] == 0x05:
        send_arr = bytearray([0xAA, 0x06, 0x00, 0xB0]) 
        send_arr = struct.pack("%dB"%len(send_arr), *send_arr)
        print(f"Sending response: {send_arr.hex()}")
        ser_send.write(send_arr)
        if act_state == 0:
            send_arr = bytearray([0xAA, 0x03, 0x00, 0xAD]) 
            send_arr = struct.pack("%dB"%len(send_arr), *send_arr)
        # flag = 1
    if data and data[0] == 0x08:
        detect_state = 0
        print("close emotion")
    if data and data[0] == 0x09:
        detect_state = 1
        print("Open emotion")


#通过情绪识别控制mp3，时间为间隔时间,以及每次识别次数
def emotion_mp3(time, detect_time):
    global flag
    global start_time
    global emotion
    global happy_count, sad_count, angry_count
    global detect_flag, detect_state
    # print(f"detect_state set to: {detect_state}")
    if confidence >= 0.7 and detect_state == 1:
        #如果状态为1
        if detect_flag != 0:
            rospy.loginfo("START!!!DETECT!!!")
            detect_flag += 1
            if emotion == 'happy':
                happy_count += 1
                rospy.loginfo("HAPPY!!!!!!")
            elif emotion == 'sad':
                sad_count += 1
                rospy.loginfo("SAD!!!!!!")
            elif emotion == 'angry':
                angry_count += 1
                rospy.loginfo("angry!!!!")
                
            if detect_flag >= detect_time:
                #rospy.loginfo("RESULT!!!!")
                start_time = rospy.get_time()
                detect_flag = 0
                count = [happy_count, sad_count, angry_count]    
                max_value = max(count)        # 找到最大值
                max_index = count.index(max_value)  # 找到最大值的索引
                if max_index == 0:
                    rospy.loginfo("RESULT_HAPPY!!!!!!")
                    send_arr = bytearray([0xAA, 0x07, 0x02, 0x00, 0x03, 0xB6])
                    send_arr = struct.pack("%dB"%len(send_arr), *send_arr)
                    ser_send.write(send_arr)
                    flag = 1
                    
                elif max_index == 1:
                    rospy.loginfo("RESULT_SAD!!!!!!")
                    send_arr = bytearray([0xAA, 0x07, 0x02, 0x00, 0x01, 0xB4])
                    send_arr = struct.pack("%dB"%len(send_arr), *send_arr)
                    ser_send.write(send_arr)
                    flag = 1
                else:
                    rospy.loginfo("RESULT_ANGRY!!!!!")
                    send_arr = bytearray([0xAA, 0x07, 0x02, 0x00, 0x02, 0xB5])
                    send_arr = struct.pack("%dB"%len(send_arr), *send_arr)
                    ser_send.write(send_arr)
                    flag = 1
        
        #如果状态为0
        if detect_flag == 0: 
            if rospy.get_time() - start_time >= time:
                detect_flag = 1
    if detect_state == 0:
        detect_flag = 0
        
            
def shutdown_callback():
    send_arr = bytearray([0xAA, 0x03, 0x00, 0xAD]) 
    send_arr = struct.pack("%dB"%len(send_arr), *send_arr)
    print(f"Sending response: {send_arr.hex()}")
    print("ShutDown")
    ser_send.write(send_arr)

rospy.on_shutdown(shutdown_callback)
            

#初始化节点
rospy.init_node('serial_control', anonymous=True)
rospy.Subscriber('/emotion_detect', Yolo, emotion_callback, queue_size=10)
rate = rospy.Rate(100)


#初始化串口
ser = serial.Serial('/dev/ttyS0', baudrate=9600, timeout=0.1)
ser_send = serial.Serial('/dev/ttyS4', baudrate=9600,timeout=0.1)


#初始化mp3
send_arr = bytearray([0xAA, 0x13, 0x01, 0x14, 0xD2])
send_arr = struct.pack("%dB"%len(send_arr), *send_arr)
ser_send.write(send_arr)


send_arr = bytearray([0xAA, 0x04, 0x00, 0xAE])
send_arr = struct.pack("%dB"%len(send_arr), *send_arr)
ser_send.write(send_arr)



flag = 0


while not rospy.is_shutdown():
    
    data = read_packet()
    mp3_control(data)
    rate.sleep()










